#include "BC_BodyOriTask.hpp"

template <typename T>
void BodyOriTask<T>::task(ControlFSMData<T>& data, BalanceCtrlData<T>* ctrl_data, BCoutputData<T>* BCout, int leg) {
     std::vector<T> oriR; 
    T R;

    // estimate
    auto& seResult = data._stateEstimator->getResult();

    // calcurate R
    oriR.clear();
    for(int leg = 0; leg < 4; leg ++){
        if(seResult.contactEstimate(leg) > 0.){
            Vec3<T> r = data._quadruped->getHipLocation(leg) + data._legController->datas[leg].p;
            T r_plant = sqrt(pow(r(0), 2) + pow(r(1), 2));
            oriR.push_back(r_plant);
        }
    }
    R = 0;
    for(int i = 0; i < oriR.size(); i ++) R += oriR[i];
    R /= oriR.size();

    // ori
    Vec3<T> delta = Vec3<T>::Zero();
    delta(2) = ctrl_data->vBody_Ori_des(2);
    Vec3<T> vDes = R * delta;

    // posture
    delta = Vec3<T>::Zero();
    delta(2) = R * sin(ctrl_data->pBody_RPY_des(2) - seResult.rpy(2));

    // p
    Vec3<T> pDes =  seResult.rBody * (ctrl_data->pFoot_des[leg] - seResult.position)  
                - data._quadruped->getHipLocation(leg) + delta;
    Vec3<T> qDes;

    // convert
    computeLegAngle(*data._quadruped, pDes, &qDes);

    // convert
    BCout->qDes = qDes;
    BCout->qdDes = data._legController->datas[leg].J.inverse() * vDes;
    // printf("pDes: %f, %f, %f\n", pDes(0), pDes(1), pDes(2));
    // printf("vDes: %f, %f, %f\n", vDes(0), vDes(1), vDes(2));

}

template class BodyOriTask<float>;
template class BodyOriTask<double>;
